import { ArcRotateCamera } from "@babylonjs/core/Cameras/arcRotateCamera";
import { Quaternion, Vector3 } from "@babylonjs/core/Maths/math.vector";
import "@babylonjs/core/Culling/ray";
import { MeshBuilder } from "@babylonjs/core/Meshes/meshBuilder";
import { BabylonContainer } from "babylon-lib/index";
import { Mesh } from "@babylonjs/core/Meshes/mesh";
import { Stage } from 'babylon-lib/base/stage';
import { AbstractCharacter } from "../abstract-character";
import { TransformNode } from "@babylonjs/core/Meshes/transformNode";
import { PhysicsCharacterController, CharacterSupportedState, CharacterSurfaceInfo } from '@babylonjs/core/Physics/v2/characterController';
import { State } from 'babylon-lib/misc/state';
import { CharacterControllerInput } from "./character-controller-input";
import { CreateHandleTo, IHandle, RemoveHandle } from "babylon-lib/data/data-handle";
import { PhysicsShapeCapsule, PhysicsShapeContainer } from "@babylonjs/core/Physics/v2/physicsShape";


enum STATES {
    IN_AIR = 'IN_AIR',
    ON_GROUND = 'ON_GROUND',
    START_JUMP = 'START_JUMP',
}

interface ICharacterControllerOptions {
    characterPosition?: Vector3;
    impostorHeight?: number;
    impostorRadius?: number;
    mass?: number;
    acceleration?: number;
    friction?: number;
    walkSpeed?: number;
    runSpeed?: number;
    jumpHeight?: number;
    characterGravity?: Vector3;
    mode?: CONTROLLER_TYPE,
    characterOffset?: Vector3;
    disablePhysicalEffects?: boolean;
    hideCharacter?: boolean;
    flySpeed?: number;

    thirdPersonCameraOffset?: Vector3;
    thirdPersonRadius?: number;
    thirdPersonRadiusLowerLimit?: number;
    thirdPersonRadiusUpperLimit?: number;
    thirdPersonAlpha?: number;
    thirdPersonAlphaLowerLimit?: number;
    thirdPersonAlphaUpperLimit?: number;
    thirdPersonBeta?: number;
    thirdPersonBetaLowerLimit?: number;
    thirdPersonBetaUpperLimit?: number;

    firstPersonCameraOffset?: Vector3;
    firstPersonRadius?: number;
    firstPersonAlpha?: number;
    firstPersonAlphaLowerLimit?: number;
    firstPersonAlphaUpperLimit?: number;
    firstPersonBeta?: number;
    firstPersonBetaLowerLimit?: number;
    firstPersonBetaUpperLimit?: number;

    flyCameraOffset?: Vector3;
    flyRadius?: number;
    flyRadiusLowerLimit?: number;
    flyRadiusUpperLimit?: number;
    flyAlpha?: number;
    flyAlphaLowerLimit?: number;
    flyAlphaUpperLimit?: number;
    flyBeta?: number;
    flyBetaLowerLimit?: number;
    flyBetaUpperLimit?: number;
}

enum INPUT_STATE {
    FORWARD,
    BACKWARD,
    LEFT,
    RIGHT,
    JUMP,
    UP,
    DOWN,
    RUN,
}

enum INPUT_TYPE {
    IDLE = 'idle',
    WALK = 'walk',
    RUN = 'RUN',
    JUMP = 'jump',
}

enum CONTROLLER_TYPE {
    FIRST_PERSON = 'firstPerson',
    THIRD_PERSON = 'thirdPerson',
    FLY = 'fly',
}


class CharacterController {

    public readonly stage: Stage;
    public readonly camera: ArcRotateCamera;
    public onInputChange?: { (type: INPUT_TYPE): void };

    private _options: ICharacterControllerOptions;
    private _impostorMesh: Mesh;
    private _character?: AbstractCharacter;
    private _physicsController: PhysicsCharacterController;
    private _state: STATES = STATES.IN_AIR;
    private _wantJump = false;
    private _inputDirection = new Vector3(0, 0, 0);
    private _forwardLocalSpace = new Vector3(0, 0, 1);
    private _characterOrientation = Quaternion.Identity();
    private _upWorld = new Vector3();
    private _gScale = new Vector3();
    private _camDir = new Vector3();
    private _down = new Vector3();
    private _inputState: State = new State();
    private _cameraTarget!: TransformNode;
    private _translateHandles: IHandle<number>[] = [];
    private _rotateHandles: IHandle<number>[] = [];
    private _scaleHandles: IHandle<number>[] = [];//....................todo:暂无实际功能，如有需要再扩充
    private _shapeContainer!: PhysicsShapeContainer;
    private _isRunning = false;
    private _airSpeed = 0;

    constructor(camera: ArcRotateCamera, stage: Stage, options?: ICharacterControllerOptions) {
        this._options = this.setOptions(options);

        this.stage = stage;
        this.camera = camera;
        this._impostorMesh = this.initializeImpostorMesh();
        this._physicsController = this.initializePhysicsController();
        this.bindEvent();
        this.bindCamera();
        CharacterControllerInput.attach(this);
        this.mode = this._options.mode!;
    }

    private setOptions(options?: ICharacterControllerOptions): ICharacterControllerOptions {
        const _options = options || {};
        return {
            characterPosition: _options.characterPosition || new Vector3(0, 1.25, 0),
            impostorHeight: _options.impostorHeight || 2,
            impostorRadius: _options.impostorRadius || 0.4,
            mass: _options.mass || 0,
            acceleration: _options.acceleration || 0.15,
            friction: _options.friction || 0,
            walkSpeed: _options.walkSpeed || 2.2,
            runSpeed: _options.runSpeed || 5,
            jumpHeight: _options.jumpHeight || 1,
            characterGravity: _options.characterGravity || new Vector3(0, -10, 0),
            mode: _options.mode || CONTROLLER_TYPE.THIRD_PERSON,
            characterOffset: _options.characterOffset || new Vector3(0, -1, 0),
            disablePhysicalEffects: _options.disablePhysicalEffects || false,
            hideCharacter: _options.hideCharacter || false,
            flySpeed: _options.flySpeed || 4,

            thirdPersonCameraOffset: _options.thirdPersonCameraOffset || new Vector3(0, 0.5, 0),
            thirdPersonRadius: _options.thirdPersonRadius || 3,
            thirdPersonRadiusLowerLimit: _options.thirdPersonRadiusLowerLimit || 1,
            thirdPersonRadiusUpperLimit: _options.thirdPersonRadiusUpperLimit || 10,
            thirdPersonAlpha: _options.thirdPersonAlpha || 0,
            thirdPersonAlphaLowerLimit: _options.thirdPersonAlphaLowerLimit || Number.NEGATIVE_INFINITY,
            thirdPersonAlphaUpperLimit: _options.thirdPersonAlphaUpperLimit || Number.POSITIVE_INFINITY,
            thirdPersonBeta: _options.thirdPersonBeta || Math.PI * 0.4,
            thirdPersonBetaLowerLimit: _options.thirdPersonBetaLowerLimit || 0,
            thirdPersonBetaUpperLimit: _options.thirdPersonBetaUpperLimit || Math.PI / 2,

            firstPersonCameraOffset: _options.firstPersonCameraOffset || new Vector3(0, 0.8, 0),
            firstPersonRadius: _options.firstPersonRadius || 0.1,
            firstPersonAlpha: _options.firstPersonAlpha || 0,
            firstPersonAlphaLowerLimit: _options.firstPersonAlphaLowerLimit || Number.NEGATIVE_INFINITY,
            firstPersonAlphaUpperLimit: _options.firstPersonAlphaUpperLimit || Number.POSITIVE_INFINITY,
            firstPersonBeta: _options.firstPersonBeta || Math.PI * 0.5,
            firstPersonBetaLowerLimit: _options.firstPersonBetaLowerLimit || 1e-6,
            firstPersonBetaUpperLimit: _options.firstPersonBetaUpperLimit || Math.PI,

            flyCameraOffset: _options.flyCameraOffset || new Vector3(0, 0, 0),
            flyRadius: _options.flyRadius || 3,
            flyRadiusLowerLimit: _options.flyRadiusLowerLimit || 0.1,
            flyRadiusUpperLimit: _options.flyRadiusUpperLimit || 100,
            flyAlpha: _options.flyAlpha || 0,
            flyAlphaLowerLimit: _options.flyAlphaLowerLimit || Number.NEGATIVE_INFINITY,
            flyAlphaUpperLimit: _options.flyAlphaUpperLimit || Number.POSITIVE_INFINITY,
            flyBeta: _options.flyBeta || Math.PI * 0.5,
            flyBetaLowerLimit: _options.flyBetaLowerLimit || 1e-6,
            flyBetaUpperLimit: _options.flyBetaUpperLimit || Math.PI,
        };
    }

    private bindEvent(): void {
        const scene = this.stage.mainScene;
        const camera = this.camera;
        const controller = this._physicsController;

        const tmpTranslate = new Vector3(1e6, 1e6, 1e6);
        let pp: Vector3;
        scene.onBeforeRenderObservable.add((scene) => {
            pp = this._physicsController.getPosition();
            this._impostorMesh.position.copyFrom(pp);
            if (Vector3.DistanceSquared(pp, tmpTranslate) > 1e-6) {
                this._translateHandles.forEach(handle => { handle.callback(tmpTranslate.x, tmpTranslate.y, tmpTranslate.z) });
            }
            tmpTranslate.copyFrom(pp);
        });

        const tmpDirection = new Vector3();
        this.stage.mainScene.onAfterPhysicsObservable.add(() => {
            if (scene.deltaTime == undefined) return;
            let dt = scene.deltaTime * 0.001;
            if (dt == 0) return;

            let down = this._down.set(0, -1, 0);
            let support = controller.checkSupport(dt, down);
            if (this._options.mode !== CONTROLLER_TYPE.FLY) {
                camera.getDirectionToRef(BabylonContainer.forward, this._camDir);
                this._camDir.y = 0;
                this._camDir.normalize();
                Quaternion.FromLookDirectionLHToRef(this._camDir, Vector3.UpReadOnly, this._characterOrientation);
            } else {
                Quaternion.FromRotationMatrixToRef(camera.getWorldMatrix(), this._characterOrientation);
            }

            const desiredLinearVelocity = this.getDesiredVelocity(dt, support, this._characterOrientation, controller.getVelocity());
            controller.setVelocity(desiredLinearVelocity);
            controller.integrate(dt, support, this._options.mode === CONTROLLER_TYPE.FLY ? Vector3.ZeroReadOnly : this._options.characterGravity!);

            if (this._character) {
                this._character.setDirection(desiredLinearVelocity);
                if (Vector3.DistanceSquared(tmpDirection, desiredLinearVelocity) > 1e-6) {
                    this._rotateHandles.forEach(handle => { handle.callback(desiredLinearVelocity.x, desiredLinearVelocity.y, desiredLinearVelocity.z) });
                }
                tmpDirection.copyFrom(desiredLinearVelocity);
            }
        });

        this._inputState.onChange(() => {
            if (this._inputState.exist(INPUT_STATE.FORWARD) && this._inputState.notExist(INPUT_STATE.BACKWARD)) this._inputDirection.z = -1;
            if (this._inputState.exist(INPUT_STATE.BACKWARD) && this._inputState.notExist(INPUT_STATE.FORWARD)) this._inputDirection.z = 1;
            if (this._inputState.exist(INPUT_STATE.LEFT) && this._inputState.notExist(INPUT_STATE.RIGHT)) this._inputDirection.x = -1;
            if (this._inputState.exist(INPUT_STATE.RIGHT) && this._inputState.notExist(INPUT_STATE.LEFT)) this._inputDirection.x = 1;
            if (this._inputState.exist(INPUT_STATE.UP) && this._inputState.notExist(INPUT_STATE.DOWN)) this._inputDirection.y = 1;
            if (this._inputState.exist(INPUT_STATE.DOWN) && this._inputState.notExist(INPUT_STATE.UP)) this._inputDirection.y = -1;

            if (this._inputState.notExist(INPUT_STATE.UP, INPUT_STATE.DOWN)) this._inputDirection.y = 0;
            if (this._inputState.notExist(INPUT_STATE.FORWARD, INPUT_STATE.BACKWARD)) this._inputDirection.z = 0;
            if (this._inputState.notExist(INPUT_STATE.LEFT, INPUT_STATE.RIGHT)) this._inputDirection.x = 0;

            if (this._inputState.exist(INPUT_STATE.JUMP)) this._wantJump = true;
            if (this._inputState.notExist(INPUT_STATE.JUMP)) this._wantJump = false;
            if (this._inputState.exist(INPUT_STATE.RUN)) this._isRunning = true;
            if (this._inputState.notExist(INPUT_STATE.RUN)) this._isRunning = false;

            this.updateCharacterStatus();
        });
    }

    private updateCharacterStatus(): void {
        if (!this.onInputChange) return;

        if (this._state === STATES.START_JUMP) {
            this.onInputChange(INPUT_TYPE.JUMP);
        } else if (this._state === STATES.IN_AIR) {
            //
        } else {
            if (this._inputDirection.x !== 0 || this._inputDirection.z !== 0) {
                this._isRunning ? this.onInputChange(INPUT_TYPE.RUN) : this.onInputChange(INPUT_TYPE.WALK);
            } else {
                this.onInputChange(INPUT_TYPE.IDLE);
            }
        }
    }

    public set debug(debug: boolean) {
        this._impostorMesh.isVisible = debug;
    }

    public set mode(mode: CONTROLLER_TYPE) {
        this._inputState.clear();
        const camera = this.camera;
        const options = this._options;
        options.mode = mode;
        const alpha = camera.alpha;
        const beta = camera.beta;

        if (mode === CONTROLLER_TYPE.FIRST_PERSON) {
            camera.radius = options.firstPersonRadius!;
            camera.lowerRadiusLimit = options.firstPersonRadius! - 1e-6;
            camera.upperRadiusLimit = options.firstPersonRadius! + 1e-6;

            camera.lowerAlphaLimit = options.firstPersonAlphaLowerLimit!;
            camera.upperAlphaLimit = options.firstPersonAlphaUpperLimit!;
            const _alpha = Math.max(Math.min(options.firstPersonAlphaUpperLimit!, alpha), options.firstPersonAlphaLowerLimit!);
            camera.alpha = _alpha;

            camera.lowerBetaLimit = options.firstPersonBetaLowerLimit!;
            camera.upperBetaLimit = options.firstPersonBetaUpperLimit!;
            const _beta = Math.max(Math.min(options.firstPersonBetaUpperLimit!, beta), options.firstPersonBetaLowerLimit!);
            camera.beta = _beta;

        } else if (mode === CONTROLLER_TYPE.THIRD_PERSON) {
            camera.radius = options.thirdPersonRadius!;
            camera.lowerRadiusLimit = options.thirdPersonRadiusLowerLimit!;
            camera.upperRadiusLimit = options.thirdPersonRadiusUpperLimit!;

            camera.lowerAlphaLimit = options.thirdPersonAlphaLowerLimit!;
            camera.upperAlphaLimit = options.thirdPersonAlphaUpperLimit!;
            const _alpha = Math.max(Math.min(options.thirdPersonAlphaUpperLimit!, alpha), options.thirdPersonAlphaLowerLimit!);
            camera.alpha = _alpha;

            camera.lowerBetaLimit = options.thirdPersonBetaLowerLimit!;
            camera.upperBetaLimit = options.thirdPersonBetaUpperLimit!;
            const _beta = Math.max(Math.min(options.thirdPersonBetaUpperLimit!, beta), options.thirdPersonBetaLowerLimit!);
            camera.beta = _beta;
        } else if (mode === CONTROLLER_TYPE.FLY) {
            camera.radius = options.flyRadius!;
            camera.lowerRadiusLimit = options.flyRadiusLowerLimit!;
            camera.upperRadiusLimit = options.flyRadiusUpperLimit!;

            camera.lowerAlphaLimit = options.flyAlphaLowerLimit!;
            camera.upperAlphaLimit = options.flyAlphaUpperLimit!;
            const _alpha = Math.max(Math.min(options.flyAlphaUpperLimit!, alpha), options.flyAlphaLowerLimit!);
            camera.alpha = _alpha;

            camera.lowerBetaLimit = options.flyBetaLowerLimit!;
            camera.upperBetaLimit = options.flyBetaUpperLimit!;
            const _beta = Math.max(Math.min(options.flyBetaUpperLimit!, beta), options.flyBetaLowerLimit!);
            camera.beta = _beta;
        }
        this.checkCharacterVisible();
    }

    public set disablePhysicalEffects(isDisable: boolean) {
        this._options.disablePhysicalEffects = isDisable;
        this.setCollideShape(!isDisable);
    }

    public set direction(direction: Vector3) {
        this._inputDirection.copyFrom(direction);
        if (!this.onInputChange) return;
        if (direction.x !== 0 || direction.z !== 0) {
            this._isRunning ? this.onInputChange(INPUT_TYPE.RUN) : this.onInputChange(INPUT_TYPE.WALK);
        } else {
            this.onInputChange(INPUT_TYPE.IDLE);
        }
    }

    public set hideCharacter(isHide: boolean) {
        this._options.hideCharacter = isHide;
        this.checkCharacterVisible();
    }

    public onTranslate(func: { (x: number, y: number, z: number): void }): IHandle<number> {
        return CreateHandleTo<number>(func, this._translateHandles);
    }

    public removeTranslateHandle(handle: number | IHandle<number>): void {
        RemoveHandle(handle, this._translateHandles);
    }

    public onRotate(func: { (x: number, y: number, z: number): void }): IHandle<number> {
        return CreateHandleTo<number>(func, this._rotateHandles);
    }

    public removeRotateHandle(handle: number | IHandle<number>): void {
        RemoveHandle(handle, this._rotateHandles);
    }

    public onScale(func: { (x: number, y: number, z: number): void }): IHandle<number> {
        return CreateHandleTo<number>(func, this._scaleHandles);
    }

    public removeScaleHandle(handle: number | IHandle<number>): void {
        RemoveHandle(handle, this._scaleHandles);
    }

    public set forward(active: boolean) {
        active ? this._inputState.add(INPUT_STATE.FORWARD) : this._inputState.remove(INPUT_STATE.FORWARD);
    }

    public set backward(active: boolean) {
        active ? this._inputState.add(INPUT_STATE.BACKWARD) : this._inputState.remove(INPUT_STATE.BACKWARD);
    }

    public set left(active: boolean) {
        active ? this._inputState.add(INPUT_STATE.LEFT) : this._inputState.remove(INPUT_STATE.LEFT);
    }

    public set right(active: boolean) {
        active ? this._inputState.add(INPUT_STATE.RIGHT) : this._inputState.remove(INPUT_STATE.RIGHT);
    }

    public set run(active: boolean) {
        active ? this._inputState.add(INPUT_STATE.RUN) : this._inputState.remove(INPUT_STATE.RUN);
    }

    public set jump(active: boolean) {
        if (this._options.mode === CONTROLLER_TYPE.FLY) return;
        active ? this._inputState.add(INPUT_STATE.JUMP) : this._inputState.remove(INPUT_STATE.JUMP);
    }

    public set up(active: boolean) {
        if (this._options.mode !== CONTROLLER_TYPE.FLY) return;
        active ? this._inputState.add(INPUT_STATE.UP) : this._inputState.remove(INPUT_STATE.UP);
    }

    public set down(active: boolean) {
        if (this._options.mode !== CONTROLLER_TYPE.FLY) return;
        active ? this._inputState.add(INPUT_STATE.DOWN) : this._inputState.remove(INPUT_STATE.DOWN);
    }

    public setCharacter(character: AbstractCharacter, offset?: Vector3): void {
        character.container.parent = this._impostorMesh;
        this._character = character;
        character.controller = this;
        if (offset) this._options.characterOffset!.copyFrom(offset);
        this.checkCharacterVisible();
    }

    public get characterOffset(): Vector3 {
        return this._options.characterOffset!;
    }

    private checkCharacterVisible(): void {
        if (!this._character) return;
        if (this._options.mode == CONTROLLER_TYPE.FIRST_PERSON) {
            this._character.container.position.set(0, Number.POSITIVE_INFINITY, 0);
            this._cameraTarget.position.copyFrom(this._options.firstPersonCameraOffset!);
        } else {
            if (this._options.hideCharacter) {
                this._character.container.position.set(0, Number.POSITIVE_INFINITY, 0);
            } else {
                this._character.container.position.copyFrom(this._options.characterOffset!);
                this._cameraTarget.position.copyFrom(this._options.thirdPersonCameraOffset!);
            }
        }
    }

    private bindCamera(): void {
        const cameraPoint = new TransformNode("CharacterControllerCameraPoint", this.stage.mainScene);
        cameraPoint.parent = this._impostorMesh;
        this.camera.setTarget(cameraPoint);
        this._cameraTarget = cameraPoint;
        this.checkCharacterVisible();
    }

    private setCollideShape(isAdd: boolean): void {
        if (isAdd) {
            const hf = (this._options.impostorHeight! - this._options.impostorRadius! * 2) * 0.5;
            const shape = new PhysicsShapeCapsule(new Vector3(0, -hf, 0), new Vector3(0, hf, 0), this._options.impostorRadius!, this.stage.mainScene);
            this._shapeContainer.addChild(shape);
        } else {
            const count = this._shapeContainer.getNumChildren();
            if (count) {
                for (let i = 0; i < count; i++) {
                    this._shapeContainer.removeChild(0);
                }
            }
        }
    }

    private initializePhysicsController(): PhysicsCharacterController {
        this._shapeContainer = new PhysicsShapeContainer(this.stage.mainScene);
        this.setCollideShape(!this._options.disablePhysicalEffects);
        const ctl = new PhysicsCharacterController(this._options.characterPosition!, {
            shape: this._shapeContainer,
        }, this.stage.mainScene);
        ctl.characterMass = this._options.mass!;
        ctl.staticFriction = this._options.friction!;
        ctl.acceleration = this._options.acceleration!;
        return ctl;
    }

    private initializeImpostorMesh(): Mesh {
        const m = MeshBuilder.CreateCapsule("CharacterControllerImpostorMesh", { height: this._options.impostorHeight!, radius: this._options.impostorRadius! }, this.stage.mainScene);
        m.visibility = 0.5;
        m.rotationQuaternion = Quaternion.Identity();
        m.position.copyFrom(this._options.characterPosition!);
        m.isVisible = false;
        return m;
    }

    private getNextState(supportInfo: CharacterSurfaceInfo): STATES {
        if (this._state == STATES.IN_AIR) {
            if (supportInfo.supportedState == CharacterSupportedState.SUPPORTED) {
                return STATES.ON_GROUND;
            }
            return STATES.IN_AIR;
        } else if (this._state == STATES.ON_GROUND) {
            if (supportInfo.supportedState != CharacterSupportedState.SUPPORTED) {
                return STATES.IN_AIR;
            }

            if (this._wantJump) {
                return STATES.START_JUMP;
            }
            return STATES.ON_GROUND;
        } else if (this._state == STATES.START_JUMP) {
            return STATES.IN_AIR;
        }
        return STATES.IN_AIR;
    }

    private getDesiredVelocity(deltaTime: number, supportInfo: CharacterSurfaceInfo, characterOrientation: Quaternion, currentVelocity: Vector3): Vector3 {
        if (this._options.mode === CONTROLLER_TYPE.FLY) {
            return this._inputDirection.scale(this._options.flySpeed!).applyRotationQuaternion(characterOrientation);
        }

        const nextState = this.getNextState(supportInfo);
        if (nextState != this._state) {
            this._state = nextState;
            this.updateCharacterStatus();
        }

        const upWorld = this._options.characterGravity!.normalizeToRef(this._upWorld);
        upWorld.scaleInPlace(-1.0);
        let forwardWorld = this._forwardLocalSpace.applyRotationQuaternion(characterOrientation);
        if (this._state == STATES.IN_AIR) {
            const desiredVelocity = this._inputDirection.scale(this._airSpeed).applyRotationQuaternion(characterOrientation);
            const outputVelocity = this._physicsController.calculateMovement(deltaTime, forwardWorld, upWorld, currentVelocity, Vector3.ZeroReadOnly, desiredVelocity, upWorld);
            outputVelocity.addInPlace(upWorld.scale(-outputVelocity.dot(upWorld)));
            outputVelocity.addInPlace(upWorld.scale(currentVelocity.dot(upWorld)));
            outputVelocity.addInPlace(this._options.characterGravity!.scaleToRef(deltaTime, this._gScale));
            return outputVelocity;
        } else if (this._state == STATES.ON_GROUND) {
            const spd = this._isRunning ? this._options.runSpeed! : this._options.walkSpeed!;
            const desiredVelocity = this._inputDirection.scale(spd).applyRotationQuaternion(characterOrientation);
            let outputVelocity = this._physicsController.calculateMovement(deltaTime, forwardWorld, supportInfo.averageSurfaceNormal, currentVelocity, supportInfo.averageSurfaceVelocity, desiredVelocity, upWorld);
            outputVelocity.subtractInPlace(supportInfo.averageSurfaceVelocity);
            if (outputVelocity.dot(upWorld) > 1e-3) {
                const velLen = outputVelocity.length();
                outputVelocity.normalizeFromLength(velLen);
                const horizLen = velLen / supportInfo.averageSurfaceNormal.dot(upWorld);
                const c = supportInfo.averageSurfaceNormal.cross(outputVelocity);
                outputVelocity = c.cross(upWorld);
                outputVelocity.scaleInPlace(horizLen);
            }
            outputVelocity.addInPlace(supportInfo.averageSurfaceVelocity);
            return outputVelocity;
        } else if (this._state == STATES.START_JUMP) {
            this._airSpeed = this._isRunning ? this._options.runSpeed! : this._options.walkSpeed!;
            const u = Math.sqrt(2 * this._options.characterGravity!.length() * this._options.jumpHeight!);
            const curRelVel = currentVelocity.dot(upWorld);
            return currentVelocity.add(upWorld.scale(u - curRelVel));
        }
        return Vector3.ZeroReadOnly;
    }

    public dispose() {
        this._impostorMesh.dispose();
    }
}


export {
    CharacterController, INPUT_TYPE, CONTROLLER_TYPE,
};